#include <stdio.h>
#include <stdlib.h>
#include <math.h>

// 卡尔曼滤波器结构体 - 包含位置和速度状态
typedef struct {
    // 状态变量: [位置, 速度]
    float x;  // 位置
    float v;  // 速度

    // 状态协方差矩阵
    float P[2][2];

    // 过程噪声协方差
    float Q[2][2];

    // 测量噪声方差
    float R;

    // 采样时间间隔
    float dt;
} KalmanFilter;

// 初始化卡尔曼滤波器
void kalmanInit(KalmanFilter *kf, float initial_x, float initial_v, 
               float dt, float Q, float R)
{
    // 初始化状态
    kf->x = initial_x;  // 初始位置
    kf->v = initial_v;  // 初始速度
    kf->dt = dt;        // 采样时间间隔

    // 初始化状态协方差矩阵
    kf->P[0][0] = 1.0f;
    kf->P[0][1] = 0.0f;
    kf->P[1][0] = 0.0f;
    kf->P[1][1] = 1.0f;

    // 初始化过程噪声协方差矩阵
    // 基于采样时间和过程噪声
    kf->Q[0][0] = (dt*dt*dt*dt)/4.0f * Q;
    kf->Q[0][1] = (dt*dt*dt)/2.0f * Q;
    kf->Q[1][0] = (dt*dt*dt)/2.0f * Q;
    kf->Q[1][1] = (dt*dt) * Q;

    // 初始化测量噪声方差
    kf->R = R;
}

// 卡尔曼滤波预测步骤
void kalmanPredict(KalmanFilter *kf)
{
    // 预测新状态
    // 位置 = 位置 + 速度 * 时间
    // 速度保持不变(假设加速度为 0)
    float predicted_x = kf->x + kf->v * kf->dt;
    float predicted_v = kf->v;

    // 更新状态协方差矩阵
    // P = F * P * F^T + Q
    float F[2][2] = {
        {1.0f, kf->dt},
        {0.0f, 1.0f}
    };

    float P_temp[2][2];

    // 计算 F * P
    P_temp[0][0] = F[0][0] * kf->P[0][0] + F[0][1] * kf->P[1][0];
    P_temp[0][1] = F[0][0] * kf->P[0][1] + F[0][1] * kf->P[1][1];
    P_temp[1][0] = F[1][0] * kf->P[0][0] + F[1][1] * kf->P[1][0];
    P_temp[1][1] = F[1][0] * kf->P[0][1] + F[1][1] * kf->P[1][1];

    // 加上 Q
    kf->P[0][0] = P_temp[0][0] + kf->Q[0][0];
    kf->P[0][1] = P_temp[0][1] + kf->Q[0][1];
    kf->P[1][0] = P_temp[1][0] + kf->Q[1][0];
    kf->P[1][1] = P_temp[1][1] + kf->Q[1][1];

    // 更新状态
    kf->x = predicted_x;
    kf->v = predicted_v;
}

// 卡尔曼滤波更新步骤
void kalmanUpdate(KalmanFilter *kf, float measurement)
{
    // 计算卡尔曼增益 K = P * H^T / (H * P * H^T + R)
    // H是测量矩阵 [1, 0]
    float H[2] = {1.0f, 0.0f};

    // 计算 H * P
    float HP[2];
    HP[0] = H[0] * kf->P[0][0] + H[1] * kf->P[0][1];
    HP[1] = H[0] * kf->P[1][0] + H[1] * kf->P[1][1];

    // 计算 H * P * H^T + R
    float HPH = HP[0] * H[0] + HP[1] * H[1] + kf->R;

    // 计算 K = P * H^T / HPH
    float K[2];
    K[0] = (kf->P[0][0] * H[0] + kf->P[0][1] * H[1]) / HPH;
    K[1] = (kf->P[1][0] * H[0] + kf->P[1][1] * H[1]) / HPH;

    // 更新状态估计
    float error = measurement - (H[0] * kf->x + H[1] * kf->v);
    kf->x += K[0] * error;
    kf->v += K[1] * error;

    // 更新状态协方差矩阵 P = (I - K*H) * P
    float IKH[2][2];
    IKH[0][0] = 1.0f - K[0] * H[0];
    IKH[0][1] = 0.0f - K[0] * H[1];
    IKH[1][0] = 0.0f - K[1] * H[0];
    IKH[1][1] = 1.0f - K[1] * H[1];

    float P_temp[2][2];
    P_temp[0][0] = IKH[0][0] * kf->P[0][0] + IKH[0][1] * kf->P[1][0];
    P_temp[0][1] = IKH[0][0] * kf->P[0][1] + IKH[0][1] * kf->P[1][1];
    P_temp[1][0] = IKH[1][0] * kf->P[0][0] + IKH[1][1] * kf->P[1][0];
    P_temp[1][1] = IKH[1][0] * kf->P[0][1] + IKH[1][1] * kf->P[1][1];

    // 复制回 P 矩阵
    kf->P[0][0] = P_temp[0][0];
    kf->P[0][1] = P_temp[0][1];
    kf->P[1][0] = P_temp[1][0];
    kf->P[1][1] = P_temp[1][1];
}

// 预测未来 t 时间后的位置
float predictFuturePosition(KalmanFilter *kf, float t)
{
    // 基于当前速度和位置预测未来位置
    return kf->x + kf->v * t;
}

// 示例应用
int main()
{
    // 初始化卡尔曼滤波器
    KalmanFilter kf;
    float dt = 0.1f;                // 采样时间间隔 0.1 秒
    float process_noise = 0.1f;     // 过程噪声
    float measurement_noise = 1.0f; // 测量噪声

    // 初始状态：位置 10.0，速度 5.0
    kalmanInit(&kf, 10.0f, 5.0f, dt, process_noise, measurement_noise);

    // 模拟一些测量数据（带有噪声）
    const int num_steps = 50;
    float true_positions[num_steps];
    float measurements[num_steps];
    float filtered_positions[num_steps];
    float filtered_velocities[num_steps];

    // 生成真实位置和带噪声的测量值
    // 假设物体以初始速度 5.0 运动，并有小的加速度变化
    float true_x = 10.0f;
    float true_v = 5.0f;

    for (int i = 0; i < num_steps; i++) {
        // 模拟微小的加速度变化
        true_v += ((float)rand() / RAND_MAX) * 0.2f - 0.1f;
        true_x += true_v * dt;

        // 保存真实位置
        true_positions[i] = true_x;

        // 生成带噪声的测量值
        float noise = ((float)rand() / RAND_MAX) * 2.0f * sqrt(measurement_noise) - sqrt(measurement_noise);
        measurements[i] = true_x + noise;
    }

    // 应用卡尔曼滤波
    for (int i = 0; i < num_steps; i++) {
        // 预测步骤
        kalmanPredict(&kf);

        // 更新步骤
        kalmanUpdate(&kf, measurements[i]);

        // 保存滤波结果
        filtered_positions[i] = kf.x;
        filtered_velocities[i] = kf.v;
    }

    // 输出结果
    printf("步骤,真实位置,测量位置,滤波位置,滤波速度\n");
    for (int i = 0; i < num_steps; i++) {
        printf("%d,\t%.2f,\t%.2f,\t%.2f,\t%.2f\n",
               i, true_positions[i], measurements[i],
               filtered_positions[i], filtered_velocities[i]);
    }

    // 预测未来位置
    float future_time = 1.0f; // 预测 1 秒后的位置
    float future_position = predictFuturePosition(&kf, future_time);
    printf("\n预测未来 %.1f 秒后的位置: %.2f\n", future_time, future_position);

    future_time = 2.0f; // 预测 2 秒后的位置
    future_position = predictFuturePosition(&kf, future_time);
    printf("预测未来 %.1f 秒后的位置: %.2f\n", future_time, future_position);

    return 0;
}
